#include "PlaneProjectTransfer.h"




// ************************************
// Method:  PlaneProjectTransfer::calibrate
// Usage:   Calculate how point in camera plane can transfer to
//          point in projector plane
//          Note that 'cameraPoints' & 'projecterPoint should be both
//          5 points with 2 dimensional
// Access:  public 
// Returns: (bool) true if calculate is success
// ************************************
bool PlaneProjectTransfer::calibrate (
  IN const MyMatrix< float >& cameraPoints,
  IN const MyMatrix< float >& projecterPoints)
{
  if (cameraPoints.row_num () != 5 || cameraPoints.column_num () != 2)
    return false;
  if (projecterPoints.row_num () != 5 || projecterPoints.column_num () != 2)
    return false;

  _ceof.clear ();
  _ceof.resize (2, 5);

  MyMatrix< float > ceofMatrix (5, 6);

  // Calculate ceof of equalation for x
  for (int r = 0; r < ceofMatrix.row_num (); ++r)
  {
    ceofMatrix[r][0] = 1.0f;
    ceofMatrix[r][1] = cameraPoints[r][X];
    ceofMatrix[r][2] = cameraPoints[r][Y];
    ceofMatrix[r][3] = projecterPoints[r][X] * cameraPoints[r][X];
    ceofMatrix[r][4] = projecterPoints[r][X] * cameraPoints[r][Y];
    ceofMatrix[r][5] = projecterPoints[r][X];
  }
  if (!ceofMatrix.calc_equation (_ceof[X]))
    return false;

  ceofMatrix.clear ();
  ceofMatrix.resize (5, 6);
  // Calculate ceof of equalation for Y
  for (int r = 0; r < ceofMatrix.row_num (); ++r)
  {
    ceofMatrix[r][0] = 1.0f;
    ceofMatrix[r][1] = cameraPoints[r][X];
    ceofMatrix[r][2] = cameraPoints[r][Y];
    ceofMatrix[r][3] = projecterPoints[r][Y] * cameraPoints[r][X];
    ceofMatrix[r][4] = projecterPoints[r][Y] * cameraPoints[r][Y];
    ceofMatrix[r][5] = projecterPoints[r][Y];
  }
  if (!ceofMatrix.calc_equation (_ceof[Y]))
    return false;

  return true;
}

// ************************************
// Method:  PlaneProjectTransfer::camera_to_projector
// Usage:   Calculate the point in projector plane that meets
//          the point in camera point
//          Note that the 'cameraPoint' & 'projectPoint' should be both
//          2 dimensional
// Access:  public 
// Returns: (bool) true if success
// ************************************
bool PlaneProjectTransfer::camera_to_projector (
  IN const Array< float >& cameraPoint,
  OUT Array< float >& projectorPoint)
{
  if (cameraPoint.size () != 2)
    return false;
  
  if (projectorPoint.size () != 2)
    projectorPoint.resize (2);

  projectorPoint[X] = _ceof[X][0] + _ceof[X][1] * cameraPoint[X]
                    + _ceof[X][2] * cameraPoint[Y];
  projectorPoint[X] /= 1 - _ceof[X][3] * cameraPoint[X]
                     - _ceof[X][4] * cameraPoint[Y];
  projectorPoint[Y] = _ceof[Y][0] + _ceof[Y][1] * cameraPoint[X]
                    + _ceof[Y][2] * cameraPoint[Y];
  projectorPoint[Y] /= 1 - _ceof[Y][3] * cameraPoint[X]
                     - _ceof[Y][4] * cameraPoint[Y];
  return true;
}

// ************************************
// Method:  PlaneProjectTransfer::set_transfer_data
// Usage:   Set transfer data that used to transfer coord of points from
//          camera to projector
//          Note that 'dataToSet' must be 2 rows and 5 columns
// Access:  public 
// Returns: bool
// ************************************
bool PlaneProjectTransfer::set_transfer_data (
  const MyMatrix< float >& dataToSet)
{
  if (dataToSet.row_num () != 2 || dataToSet.column_num () != 5)
    return false;
  _ceof = dataToSet;
}

// ************************************
// Method:  PlaneProjectTransfer::get_transfer_data
// Usage:   Get transfer data
// Access:  public 
// Returns: (MyMatrix< float >) A matrix with 2 rows and 5 columns
// ************************************
MyMatrix< float > PlaneProjectTransfer::get_transfer_data ()
{
  return _ceof;
}